#include "GaussJordan.hpp"

void gaussJordan(rmatrix& A, rmatrix& R) {
	int m = Ub(A, ROW);
	int n = Ub(A, COL);
	
	// Verifies if the matrix is square
	bool ok = (m == n) ? true : false;
	int *v = new int[m];
	
	double **r = RmatrixToDouble(A);	
	
	// Vector that will control what's happening with the column
	for(int i = 0; i < m; i++){
		v[i] = i;
	}
	
	int i = 0, j = 0, k = 0, errorCode = 0, l = 0;
	int incA = 1; // for cblas_dswap
	double h = 0.0;
	
	real realH = 0.0, realAux1 = 0.0, realAux2 = 0.0, realAux3 = 0.0;

	const real eps = 1E-15;

	while ((j < m) && ok){
		// pivot search
		if (j < m){
			l = i;
			for(k = i+1; k < m; k++){
				if(abs(real(r[k][j])) > abs(real(r[l][j]))){
					l = k;
				}
			}
			
			// row interchange
			if(i != l){
				cblas_dswap(m, r[l], incA, r[i], incA);
				k = v[l]; 
				v[l] = v[i];
				v[i] = k;
			}
		}
		
		// transformation
		if(real(r[i][j]) == 0){
			ok = false;
		} else {
			realAux1 = real(r[i][j]);
			realH = 1.0 / realAux1;
			
			h = _double(realH);
			
			scalarByCol(r, m, j, h);

			r[i][j] = h;

			for(k = 0; k < m; k++){
				if(k != j){
					for(l = 0; l < m; l++){
					    if(l != i){
							realAux1 = real(r[l][k]);
							realAux2 = real(r[l][j]);
							realAux3 = real(r[i][k]);

							realH = realAux1 - realAux2 * realAux3;
							
							if(realH == 0.0) {
								realAux1 = real(r[l][k]);
								realAux1 *= eps;
								r[l][k] = _double(realAux1);
							} else {
								r[l][k] = _double(realH);
							}
						}
					}
					r[i][k] *= -r[i][j];
				}
			}
		}
		
		i++;
		j++;  // i and j run simultaniously
	}
	
	// column interchange
	if(ok){
		for(k = 0; k < m; k++){
			l = k;
		    while(v[k] != k){
				i = v[k]; 
				v[k] = v[i]; 
				v[i] = i;
				
				j = i;
				
				swapCols(r, j, l, m);
			}
		}
	}

	errorCode = !ok;
	
	for(int i = 0; i < m; i++){
		for(int j = 0; j < n; j++){
			R[i+1][j+1] = real(r[i][j]);
		}
	}
	
	
	delete [] r;
	delete [] v;
	return;
} // end gaussJordan

inline void swapCols(double **a, const int col1, const int col2, const int nRows){
	double aux=0.0;

	for(int i = 0; i < nRows; i++){
		aux = a[i][col1];
		a[i][col1] = a[i][col2];
		a[i][col2] = aux;
	}
}

inline void scalarByCol(double **a, const int nRows, const int k, const double x){
	for(int i = 0; i < nRows; i++){
		a[i][k] *= x;
	}	
}

/*
 * newIntMatrix - Aloca uma matriz de inteiros m x n.
 * 
 * Parâmetros de entrada:
 * 	- m, n: número de linhas e colunas, respect., da matriz a ser alocada.
 *
 * Parâmetros de saída: nenhum.
 *
 * Valor de retorno: ponteiro para a matriz alocada.
 *                        
 */
int **newIntMatrix(int m, int n){
	int tam = m;
	int **a = new int*[tam];
	
	for(int i = 0; i < m; i++){
		a[i] = new int[n];
	}
	
	for(int i = 0; i < m; i++){
		memset(a[i], 0, sizeof(int) * n);
	}
	
	return a;
}

/*
 * newDoubleMatrix - Aloca uma matriz de doubles m x n.
 * 
 * Parâmetros de entrada:
 * 	- m, n: número de linhas e colunas, respect., da matriz a ser alocada.
 *
 * Parâmetros de saída: nenhum.
 *
 * Valor de retorno: ponteiro para a matriz alocada.
 *                        
 */
double **newDoubleMatrix(int m, int n){
	int tam = m;
	double **a = new double*[tam];
	
	for(int i = 0; i < m; i++){
		a[i] = new double[n];
	}
	
	for(int i = 0; i < m; i++){
		memset(a[i], 0, sizeof(double) * n);
	}
	
	return a;
}

/*
 * delete_matrix - Libera uma matriz alocada dinamicamente através do operador new.
 * 
 * Parâmetros de entrada:
 * 	- **mat: referência à matriz que deve ser liberada.
 *
 * Parâmetros de saída: nenhum.
 *
 * Valor de retorno: void.
 *                        
 */
void deleteMatrix(double **mat){
	delete [] mat;
	mat = NULL;
	
	return;
}

/*
 * delete_matrix - Libera uma matriz alocada dinamicamente através do operador new.
 * 
 * Parâmetros de entrada:
 * 	- **mat: referência à matriz que deve ser liberada.
 *
 * Parâmetros de saída: nenhum.
 *
 * Valor de retorno: void.
 *                        
 */
void deleteMatrix(int **mat){
	delete [] mat;
	mat = NULL;
	
	return;
}

double** RmatrixToDouble(const rmatrix& A){
	int m = Ub(A, ROW);
	int n = Ub(A, COL);
	
	double **mat = newDoubleMatrix(m, n);
	
	for(int i = 0; i < m; i++){
		for(int j = 0; j < n; j++){
			mat[i][j] = _double(A[i+1][j+1]);
		}
	}
	
	return mat;
}
